/* system includes */
#include <string.h>
#include <stdlib.h>

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "FreeRTOSConfig.h"

#include "config.h"

#include "physics.h"
#include "wdt.h"
#include "pwm.h"
#include "adc.h"
#include "uart.h"

#include "motor.h"
#include "filter.h"
#include "accelerometer.h"
#include "gyroscope.h"
#include "led.h"
#include "imu_filter.h"
#include "kalman_filter.h"

extern uint8_t motors_on;

int32_t pitch;
int32_t roll;
int32_t yaw;

/**
 * Initialize filter
 *
 * @param  none
 * @return none
 *
 * @brief  create FreeRTOS filter task
 *
 */
void filter_init(void)
{

    /* reserve space for filter task. */
    if (pdPASS != xTaskCreate( vFilterTask, ( signed char * ) "Filter", 2*configMINIMAL_STACK_SIZE, ( void * ) NULL,
            tskIDLE_PRIORITY, NULL))
    {
        ui_printf("Couldn't create Filter Task\n");
    }
}

/**
 * Filter Task
 *
 * @param  void *pvParameters FreeRTOS parameters
 * @return none
 *
 * @brief  get Gyro and ACC values and filter the values
 *
 */
void vFilterTask(void *pvParameters)
{
    portTickType xLastWakeTime;
    portTickType last_imu_calculation;
    float update_rate;
    uint8_t i;
    /* set period to 10 ms */
    const portTickType xPeriod = (5 / portTICK_RATE_MS);

    /* initialize with current tick */
    xLastWakeTime = xTaskGetTickCount();

    gyroscope_init();

    accelerometer_init();

    imu_filter_init(0.01);

    /* this task also maintains the watchdog timer */
    wdt_init();

    /* Task main Loop */
    while (1)
    {
        /* reset wdt as long as this task keeps running */
        wdt_reset();

        /* schedule task at fixed timing */
        vTaskDelayUntil(&xLastWakeTime, xPeriod);

        /* calculate update rate according to RTOS ticks */
        update_rate = xTaskGetTickCount() - last_imu_calculation;
        /* set sample rate */
        imu_filter_set_sample_rate(update_rate/1000);
        /* get time for next calculation */
        last_imu_calculation = xTaskGetTickCount();

        /* sample gyroscope values */
        gyroscope_sample();

        /* sample accelerometer values */
        accelerometer_sample();

        /* integrate gyroscope values */
        //gyroscope_integrate_runge_kutta(0.008);

        /* update imu filter*/
        imu_filter_update(gyro.w_y, gyro.w_x, gyro.w_z, acc.a_y, acc.a_x, acc.a_z);

        /* calculate Euler angels */
        imu_filter_compute_euler();

        //ui_printf("%d %d %d\r", (int32_t) acc_get_pitch(),(int32_t) acc_get_roll(),(int32_t) acc_get_yaw());
        //ui_printf(" %d %d %d\n", (int32_t) acc.a_y, (int32_t) acc.a_x, (int32_t) acc.a_z);
        //ui_printf("%d %d %d\n", (int32_t) (gyro.w_x*1000), (int32_t) (gyro.w_y*1000), (int32_t) (gyro.w_z*1000));
        //ui_printf(" %d %d %d\r", (int32_t) (gyro.x_rungekutta*1), (int32_t) (gyro.y_rungekutta*1), (int32_t) (gyro.z_rungekutta*1));
        //ui_printf("%d %d %d %d %d %d\r", (int32_t) (gyro.x_rungekutta*1),(int32_t) acc.a_y, (int32_t) (gyro.y_rungekutta*1),(int32_t) acc.a_x, (int32_t) (gyro.z_rungekutta*1),(int32_t) acc.a_z);

        pitch = (int32_t) (1000*toDegrees(imu_filter_get_pitch()));
        roll = (int32_t) (1000*toDegrees(imu_filter_get_roll()));
        yaw = (int32_t) (1000*toDegrees(imu_filter_get_yaw()));


        if (pitch > 0)
        {
        motors[0].speed = 13000 + (uint32_t)pitch/4;
        motors[0].number = 0x02;
        motor_set_speed(&motors[0]);
        }

        if (pitch < 0)
        {
        pitch = -pitch;
        motors[1].speed = 130000 + (uint32_t)pitch/4;
        motors[1].number = 0x01;
        motor_set_speed(&motors[1]);
        }
        /* motors enabled? */
        if (motors_on)
        {
            //motor_set_speed(&motors[0]);
            //motor_set_speed(&motors[1]);
        }
        else
        {
            //motor_switch_off(&motors[0]);
            //motor_switch_off(&motors[1]);
        }
    }
}



